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COfON 


Under  the  Engineering  Analysis  Task  of  Contract  F08635-87-C-0074, 
two  development  efforts  pertaining  to  GADS  (Graphic  Attitude  Deter¬ 
mination  System)  algorithms  were  pursued.  These  efforts  resulted  in 
two  upgrades  to  the  GADS  algorithms.  The  first  upgrade  resulted  from 
a  direct  linear  algebra  solution  for  the  model  drawing  equations  for  a 
body  of  revolution.  The  second  upgrade  resulted  from  developing  an 
error  analysis  that  is  driven  by  the  model  being  displayed  to  deter¬ 
mine  an  estimated  covariance  for  the  errors  in  the  position  and  atti¬ 
tude  of  the  model  being  fit  to  the  displayed  image. 
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2.0  IMPLEMENTATION  OF  ERIM  ALGORITHM  3  FOR  MODEL  DRAWING 

From  several  of  the  project  reviews  the  model  drawing  algorithm 
discussed  here  became  known  as  ERIM  algorithm  3.  For  historical  back¬ 
ground,  ERIM  algorithm  1  used  a  linear  algebra  solution  to  find  the 
visible  edge  by  finding  the  plane  through  the  axis  of  revolution  of 
the  body  and  normal  to  the  plane  containing  the  1 ine-of-sight  and  the 
axis  of  revolution.  This  algorithm  was  used  initially  to  increase  the 
model  drawing  speed.  ERIM  algorithm  2  used  a  linear  algebra  solution 
to  compute  a  point  on  the  visible  edge  at  each  station  along  the  axis, 
by  projecting  the  1 ine-of-sight  into  the  plane  normal  to  the  axis 
revolution  and  then  computing  the  tangent  points  of  that  projection 
with  the  circle  corresponding  to  the  body.  This  corresponds  to  a 
cylindrical  approximation.  The  resulting  models  were  approximately 
equivalent  to  the  original  GADS  algorithm  and  executed  more  rapidly. 
ERIM  algorithm  3  uses  a  more  complete  linear  algebra  solution  and  is 
based  on  finding  the  1 ine-of-sight  which  is  orthogonal  to  the  surface 
normal  of  the  body  of  revolution  at  each  station  along  the  axis  of 
revolution.  This  algorithm  retains  most  of  the  speed  of  algorithm  2, 
handles  the  nearly  end-on  case  correctly,  and  provides  for  a  depth 
encoded  outline  of  the  model  to  be  presented  on  the  display. 

2.1  LINEAR  ALGEBRA  SOLUTION  FOR  TANGENT  POINT 

Assume  that  the  body  of  revolution  is  modeled  by  a  series  of  trun¬ 
cated  cones  which  have  a  common  axis  (herein  taken  to  be  the  X-axis). 
This  is  the  form  used  in  the  GADS  model  database.  An  additional 
assumption  will  be  used:  that  each  individual  "cylinder"  in  the  data 
base  is  smooth.  The  original  data  base  often  has  objects  with  steps 
or  slope  changes  combined.  The  reason  for  the  additional  assumptions 
is  to  give  the  user  control  over  how  the  visible  edges  potentially 
created  by  steps  and  slope  changes  are  displayed. 

The  linear  algebra  solution  to  the  tangent  point  of  a  1 ine-of- 
sight  with  a  truncated  cone  is  illustrated  in  Figure  1.  The  X-axis 
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("into  the  paper")  is  the  axis  of  revolution  of  the  cone.  The  sub¬ 
scripts  b  and  1  denote  body  and  lens.  The  solution  is  based  on  com¬ 
puting  the  two  lines-of-sight,  T,  (a  quadric  equation)  which  are 
orthogonal  to  the  normal  to  the  surface  of  the  truncated  cone, 

T»n  =  0  . 

The  two  solutions,  when  they  exist,  correspond  to  the  two  visible 
edges  of  the  cone  which  are  on  opposite  sides  of  the  cone.  The  visi¬ 
ble  edges  are  straight  lines  and  correspond  to  the  intersections  of 
the  cone  with  its  tangent  planes  that  pass  through  the  origin  of  the 
1 ine-of-sight.  To  derive  the  solution,  first  let  the  origin  of  the 
1 ine-of-sight  be  transformed  into  the  coordinate  system  of  the  body  of 
revolution.  The  x  component,  X5  -  X] ,  is  the  distance  from  the  lens 
to  the  plane  normal  to  the  axis  of  revolution  at  the  station,  X5,  of 
interest  along  the  axis.  The  vector  [Y-|,  Z-j ] T  is  the  projection  of 
the  line  from  the  lens  to  the  axis  of  revolution  onto  the  same  plane, 
whose  length  is  R] .  The  radius  of  the  body  of  revolution  at  Xb  is 
Rb(Xb),  where  functional  notation  will  be  dropped  for  simplicity. 

Thus  T  becomes 

Vxi 


The  surface  normal  depends  on  the  position  on  the  circle  of  revo¬ 
lution.  The  component  of  the  normal  in  the  plane  of  revolution  is 
radial  and  thus  proportional  to  the  radius  vector.  The  component  in 
the  x-direction  is  directly  proportional  to  both  the  radius  and  the 
slope  of  the  cone.  Thus  n  becomes, 

n  W] 

“Kb  dx 

Yb 

Zb 
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Setting  T»n  =  0  gives 

dRh 

0  =  '<xb  -  xl>Rb  -dT  +  'Yb  -  Yl>Yb  +  <zb  -  zl>zb 

Because  there  are  two  unknowns  Y]  and  Z],  a  second  equation  is  needed. 
The  radius  of  revolution  provides  the  needed  constraint, 

Rb2  =  Yb2  +  Zb2. 

Expressing  the  position  on  the  circle  of  revolution  in  terms  of  com¬ 
ponents  normal  and  col  inear  with  the  projection  of  the  1 ine-of-sight 
onto  the  plane  of  revolution  gives 


b  R 


D 

=  p-  '  sin  l  +  cos  ft 


where  l  is  the  angle  between  the  lines  from  [Xb,  Y] ,  Zq ] T  (the  pro¬ 
jected  origin  of  the  1 ine-of-sight)  to  [Xb,  0,  0]T  (the  center  of  the 
circle  of  revolution)  and  to  [Xb,  Yb,  Zb]T  (the  tangent  point). 

Combining  these  equations  gives  the  following  equations  for  sin  l 
and  cos 


QRl  *  N 

0  -  -  dT  (xb  -  xl  JRl 


»b  I.  Rl  1  f 

+  — ^  sin  ft  +  cos  l  Z^  -  p1  Y^  sin  l  Y^  +  cos  l  Z-j 

Rf  b 


Rb  f  .  Ri 

+  — ^  sin  ^  zi  "  cos  ^  Yi  "  r“  zi  sin  f  zi  "  cos  ^  Yi 
Rf  b 


0  =  - 


?b  B  2  R1  L  2 


dx  Xb  ’  X1  +  n  2  R1  "  R.  R1  sin  ^ 
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where  is  the  length  of  the  tangent  from  the  origin  of  the  line-of- 
sight  to  the  cone  extended  to  X].  If  the  value  is  imaginary,  the 
origin  of  the  line-of-sight  (viewing  point)  lies  within  the  extended 
cone  and  there  are  no  visible  edges. 

2.2  REVISED  DISPLAY  ALGORITHM 

The  revisions  to  the  model  display  algorithm  have  two  purposes: 
Some  implement  the  new  display  computations  and  some  provide  addi¬ 
tional  speed.  The  listing  of  the  CIRCLE  3  module  in  Appendix  A  con¬ 
tains  the  important  changes. 

The  general  organization  of  the  module  has  been  changed  to  perform 
the  display  calculations  in  three  passes  over  the  data  for  each 
"cylinder"  of  the  model.  The  first  pass  analyzes  each  truncated  cone 
to  determine  which  viewing  case  to  use  and  compute  intermediate 
results.  The  second  pass  computes  the  tangent  points,  marks  beginning 
and  end  of  visible  edges,  and  determines  where  to  draw  circles.  The 
third  pass  computes  the  data  structure  for  driving  the  display  rou¬ 
tines.  Before  the  first  pass,  the  position  of  the  camera  is  trans¬ 
formed  into  "cylinder"  coordinates.  The  result  is  used  to  form  "unit" 
vectors  corresponding  to  the  axis  of  revolution  [X-| ,  0,  0]T,  the  pro¬ 
jection  of  the  origin  of  the  line-of-sight  onto  the  plane  of  revolu¬ 
tion  [0,  Y],  Z]]T,  and  normal  to  that  projection  in  the  same  plane  [0, 
X],  -Yl]T.  These  "unit"  vectors  are  rotated  (not  translated)  into  the 
camera  coordinate  system.  This  allows  the  tangent  points  to  be  com¬ 
puted  directly  in  camera  coordinates,  which  saves  time  by  not  having 
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to  rotate  each  point.  The  subroutines  MM2  performs  only  rotation, 
compared  to  MM3  which  performs  both  rotation  and  translation. 

The  first  pass  analyzes  each  cone  by  computing  its  slope,  the 
radius 

f  dRh  t  ii2 

Rb  -  3T  (xb  -  xl) 

of  the  extended  cone  at  X],  and  R^.  A  total  of  seven  cases  are 
del ineated: 

1.  Inside  the  extended  cone  between  truncating  plane  and 
infinity. 

2.  Inside  the  truncated  cone. 

3.  Inside  the  extended  cone  between  truncating  plane  and  apex. 

4.  Inside  the  extended  cone  beyond  apex. 

5.  Outside  the  extended  cone  on  apex  side. 

6.  Outside  the  truncated  cone  between  truncated  planes  (includes 
the  degenerate  case  of  a  cylinder,  DRDX  =  0). 

7.  Outside  the  extended  cone  away  from  apex  side. 

The  values  of  the  slope  and  the  radius  are  extended  to  the  last  point 
of  the  model  so  that  the  second  pass  will  compute  the  second  tangent 
point  on  the  last  truncated  cone. 

The  second  pass  computes  the  points  to  be  connected  for  the  vis¬ 
ible  edge  and  where  circles  are  to  be  drawn.  Cases  3,  4,  and  5  cor¬ 
respond  to  visible  edges,  and  the  tangent  points  are  computed  using 
the  values  from  the  first  pass.  Non-zero  radius  circles  are  drawn  at 
both  ends  and  wherever  the  "case"  changes  between  visible  and  hidden 
or  between  two  lines  hidden  on  opposite  sides  of  the  extended  cone 
apex. 
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The  third  pass  computes  the  display  data  structure,  first  for  the 
visible  edges  and  then  for  the  circles.  It  also  stores  the  computed 
depth  of  each  point  for  depth  encoding  of  the  lines  on  the  display. 
This  depth  encoding  is  accomplished  by  the  Cytocomputer  after  model 
movement  stops,  when  requested  by  the  operator. 
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3.0  GENERATING  A  THEORETICALLY  BASED  ERROR  COVARIANCE  MATRIX 

The  purpose  of  this  effort  was  to  develop  a  covariance  matrix  for 
the  errors  in  position  and  orientation  estimates  obtained  using  manual 
GADS.  The  basic  underlying  assumption  made  is  that  a  human  operator 
matching  a  wireframe  model  to  an  image  is  using  the  location  of  edges 
and  is  minimizing  the  "overall"  error  of  the  match.  This  process  has 
been  modeled  by  a  least  squares  estimation  process,  using  linearized 
equations  to  relate  the  model  parameters  (position  and  orientation)  to 
displacements  of  pixels  in  the  image. 

The  wireframe  model  used  by  GADS  is  made  from  line  segments  which 
model  planar  structures  (e.g.,  fins)  and  bodies  of  revolution.  The 
bodies  of  revolution  are  modeled  as  a  smoothly  varying  series  of  trun¬ 
cated  cones,  which  result  from  rotating  a  curve  made  from  a  series  of 
line  segments.  Thus,  to  develop  the  least  squares  estimator,  we  will 
examine  the  matching  of  a  line  segment  to  features  in  an  image. 

Assume  that  the  line  segment  is  drawn  between  two  points  of  the 
model ,  Pj  and  p2  and  that  a  set  of  edge  features  were  matched  to  the 

line  at  locations  f ^ ,  i  =1,  .  .  .,  I.  Let  the  position  and  orienta¬ 
tion  parameters  of  the  model  be  denoted  as  a  vector  a, 

s T  *  [v  V  V  r'  P’  y] 

where  tx  =  Camera  x  translation,  in  inches  from  model  origin 

ty  =  Camera  y  translation,  in  inches  from  model  origin 

tz  =  Camera  z  translation,  in  inches  from  model  origin 

r  =  Model  roll  angle 
p  =  Model  pitch  angle 
y  =  Model  yaw  angle 

The  position,  xc  =  [xc,  yc,  zc]^,  in  camera  coordinates  of  a  model 
point,  xm  =  [xm,  ym,  zm]^,  is  given  by 


11 


^ERIM 


c  c 

p  y 

-sp 

"cpsy 

CSC  -  s  s 
r  p  y  r  y 

crcp 

“crspsy  "  srcy 

s  s  c  +  c  s 
r  p  y  r  y 

S  C„ 

r  p 

-srspsy  +  crcy 

0 

0 

where  Sp,  Cp  denote  sine  and  cosine  of  the  subscript  p  (pitch).  A 
perspective  transformation  brings  points  in  camera  coordinates  into 
image  coordinates, 

[  x'V)*1  kt  ♦  k2 
yc(zc)_1  k3  +  k4 


where  kj_ ,  k2,  1*3,  and  k4  are  display-dependent  constants. 

In  the  matching  process  the  operator  adjusts  the  parameters,  a,  so 
that  the  line  segment  has  the  same  slope  as  the  edge  and  is  placed  on 
the  edge.  This  corresponds  to  "putting  a  straight  line"  through 
the  points  (T-j ,  i  =  1,  1)  which  represent  the  edge.  Let  ui  and  un  be 
unit  vectors  in  the  direction  of  the  line  segment  and  normal  to  it 
respectively, 


U1 


Pi  -  P2 
Pi  '  P2 


Then  the  distances  of  each  feature  point  from  the  line,  d^,  and  along 
the  line,  lj,  can  be  computed  by 


T 

di  ■ 

ft 

‘  pc. 

U1  ' 

t 

» 

< 

— H 

II 

mf— 

ft 

-  PcJ 

“n  ' 
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and 


where  pc  is  the  center  of  the  line  segment.  For  short  line  segments, 
only  the  displacement  will  be  used  now  in  the  fitting  process.  The 
analysis  can  be  expanded  to  include  slope  for  long  line  segments.  The 
least  squares  fit  to  an  individual  line  segment  of  a  displacement,  Ad, 
along  the  normal  is  given  by 


Ad 


di 


The  errors  of  this  process  depend  on  the  ability  to  locate  edge 
features.  We  will  assume  that  the  variance,  a j2,  of  the  errors  in 
locating  the  edge  (measuring  di)  is  constant  over  the  image  but  depen¬ 
dent  on  image  quality.  However,  the  dj  will  not  be  independent  for 
closely  spaced  dj.  Therefore,  we  will  assume  a  constant  decorrelation 
distance,  Xj,  along  an  edge  which  again  depends  on  image  quality.  The 
variance  of  Ad  can  then  be  expressed  approximately  as 


These  displacements  from  each  line  segment  of  the  model  are 
"averaged"  over  the  whole  model  by  the  operator  in  adjusting  the  model 
parameters,  a,  to  minimize  the  overall  error.  This  process  will  be 
represented  by  the  least  squares  fit  of  the  displacements  for  line 
segments  of  the  model  using  linearized  equations.  Let  the  displace¬ 
ments  for  all  the  line  segments  of  the  model  be  denoted  by 

Adj ,  j  =  1 ,  .  .  .,J, 

The  displacements  can  be  expressed  in  terms  of  increments  in  the  model 
parameters,  A a,  by  linearizing  about  the  estimate  of  the  model  param- 
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eters,  a.  The  resulting  set  of  equations  is 

Ad.  =  a Aa,  j  =  1,  .  .  . ,  J 

J  J 

where  aijT  is  the  product  of  the  normal  and  the  matrix  of  partial 
derivatives  of  Pc  with  respect  to  each  of  the  model  parameters, 

-  T  -  T  f^r  0Pr  8?r  & r 

j  n  at  •  at  '  at  •  ar  *  ap  •  ay  * 

x  y  z 


These  equations  can  be  weighted  to  be  unit  variance  and  combined 
into  matrix  form  as 

Ad  =  A  Aa 


where 


V'jd, 

ty'Ad. 


IV'AdJ 


V^Ad, 

a2T/irAd. 


A  =  I  • 


lV'“j 
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The  weighted  least  squares  solution  is  computed  using  the  generalized 
inverse  of  A  and  is  given  by 

Aa  =  [aTa]  1  AT  Ad  . 

Given  this  relationship  we  can  now  derive  a  covariance  based  on  small 
error  approximations. 

3.1  DERIVATION  OF  COVARIANCE  MATRIX 

The  covariance  matrix  for  the  estimated  parameters  of  the  model  is 
is  derived  from  the  weighted  least  squares  solution.  The  covariance 
of  a  is  given  by 

Cov  (a)  =  E'  [a  -  E{a}j  ^a  -  E{a}^ 


Assume  that  the  initial  guess  for  a  is  sufficiently  close  and  that 
the  fitting  process  has  been  iterated  to  a  stable  solution.  Then  the 
linearized  weighted  least  squares  solution  derived  above  can  be  used 
to  compute  the  covariance  by  taking  E{a}  as  the  point  of  lineariza¬ 
tion.  Under  theses  conditions, 


[a  -  E{a}]  =  Aa  =  [aTa]  V  Ad 
■  t 

Cov  (a)  =  E  [Aa]  ^Aaj 

»  4 


=  (aTa)  1ATe|ac1  Ad^A^A] 


■  K4 


because  the  displacements  Ad  were  constructed  to  be  a  unit  variance 
uncorrelated  random  process. 
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3.2  IMPLEMENTATION  OF  COVARIANCE  COMPUTATION  ALGORITHM 

The  algorithm  for  computing  the  covariance  is  an  enhancement  of 
the  model  drawing  algorithm.  After  the  end-points  of  a  line  segment 
to  be  drawn  are  computed,  the  contribution  to  the  inverse  of  the 
covariance  matrix  is  computed.  After  all  line  segments  are  con¬ 
sidered,  the  matrix  is  inverted  to  form  the  covariance. 

The  contribution  for  each  line  segment  is  the  outer  product  of  aj 
with  itself  normalized  by  the  length  of  the  line  segment.  Thus,  the 
incremental  contribution  to  the  inverse  covariance  is  given  by 

xdffd  cov"1(“)  =  Vd  cov_1(“)  +  |pi(j)  -  P2(j)  |  VjT 

where  pj(j)  is  the  pixel  position  of  the  beginning  of  the  line 
segment, 

Pl(j)  is  the  pixel  position  of  the  beginning  of  the  line 
segment, 

P2(J)  is  the  pixel  position  of  the  end  of  the  line  segment,  and 

aj  is  the  projection  of  the  partial  derivative  matrix  onto 
the  normal  to  the  line  segment. 

The  partial  derivatives  are  computed  using  the  chain  rule.  Thus,  the 
partial  derivatives  of  a  position  in  pixel  space  is  given  in  terms  of 
camera  coordinate  partial  derivatives  by 


where  represents  any  of  the  model  parameters.  The  partial  deriva¬ 
tives  of  the  position  in  camera  coordinates  with  respect  to  the  camera 
translations  are  constants,  either  zero  or  unity, 


16 


9 


and 


axc 

8yC 

azc 

at. 

“  at  ‘ 

at 

z 

X 

y 
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These  rotation  matrices  are  computed  using  a  variation  of  the  subrou¬ 
tine  for  the  model  transformation  matrix. 
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